-module (kinematics_test).
-export( [run/0] ).


-record( target, {pivot, accel, posCast=[]} ).

targetObjectUpdate( State = #target{}, Pos, Vel, DT ) ->
  %error_logger:info_msg( "Arc upd: ~p\n", [Pos] ),
  R = geom:vec3sub( State#target.pivot, Pos ),
  RL = geom:vec3len( R ),
  NewVel = geom:vec3ma( Vel, R, DT * State#target.accel / ( RL * RL * RL ) ),
  %error_logger:info_msg( "Pos: ~p, Add: ~p\n", [Pos, DT * Accel / ( RL * RL * RL )] ),
  [ kinematics:ctrlObj( Pid, { object_pos_upd, self(), Pos } ) || Pid <- State#target.posCast ],
  { continue, State, Pos, NewVel }.



targetObjectMsg( State = #target{posCast=Cast}, Msg, Pos, Vel ) ->
  error_logger:info_msg( "Arc msg: ~p\n", [Msg] ),
  case Msg of
    { subscribe_for_pos, Pid } ->
      { continue, State#target{ posCast=[Pid|Cast] }, Pos, Vel }
  end.



startTargetObject( KinWorld ) ->
  State = #target{ pivot={ 10.0, 20.0, 0.0 }, accel=100000.0 },
  H = random:uniform(),
  V = random:uniform(),
  Pos = {10.0, 100.0 + H * 50.0, 0.0},
  Vel = {20.0 + V * 10.0, 0.0, 0.0},
  kinematics:newObject( KinWorld, Pos, Vel, State, fun targetObjectUpdate/4, fun targetObjectMsg/4 ).



chaserObjectUpdate( State_TargetPos, Pos, _Vel, _DT ) ->
  R = geom:vec3sub( State_TargetPos, Pos ),
  Vel = geom:vec3scale( geom:vec3normalize( R ), 25.0 ),
  { continue, State_TargetPos, Pos, Vel }.



chaserObjectMsg( _State, Msg, Pos, Vel ) ->
  case Msg of
    { object_pos_upd, _TargetPid, NewTargetPos } ->
      { continue, NewTargetPos, Pos, Vel }
  end.
  
  
  
startChaserObject( KinWorld, Target ) ->
  State = geom:zeroVec3(),
  Pos = { random:uniform() * 100, random:uniform() * 100, random:uniform() * 100 },
  Chaser = kinematics:newObject( KinWorld, Pos, {0,0,0}, State, fun chaserObjectUpdate/4, fun chaserObjectMsg/4 ),
  kinematics:ctrlObj( Target, { subscribe_for_pos, Chaser } ).
  
  
  
run() ->
  Aabb = { {-200.0, -200.0, -100.0}, {200.0, 200.0, 100.0} },
  KinWorld = kinematics:newWorld( Aabb, {50, 50, 50}, 50.0 ),
  
  F = fun(_) ->
    T = startTargetObject( KinWorld ),
    startChaserObject( KinWorld, T )
  end,
  lists:foreach( F, lists:seq(1, 10) ),
  Vis = vis:start( {-200.0, -200.0, +200.0, +200.0} ),
  kinamaticsVisLoop( KinWorld, Vis ).

  

kinamaticsVisLoop( World, Vis ) ->
  kinematics:dbgVisWorld( World, Vis ),
  timer:sleep( 50 ),
  kinamaticsVisLoop( World, Vis ).
